#include "kalman.h"
#include "math.h"
#include "stdio.h"
#include "config.h"
#include "mainlogic.h"
#include "Utility.h"
#include "DBG.h"

#define ABS(x) ((x)>0?(x):(-(x)))
static  int g_preDacValue = 0;
static  int g_ppreDacValue = 0;
static  int g_preOffset = 0;
static  float g_ctvalue = 0;
static  float g_ctvalueSum = 0;
static  int g_ctvalueCount = 0;

void kalmanFilterYuce(KalmanData *data) {
	//data->yuceValue = data->yuceValue;
	//data->yuceValue = data->yuceValue + (g_ppreDacValue-g_preDacValue)*g_ctvalue;
	//printf("ppre=%d pre=%d offset = %d yucevalue = %d\r\n",g_ppreDacValue,g_preDacValue,g_ppreDacValue-g_preDacValue,data->yuceValue);
	data->yuceValue = 0;
}

double kalmanFilter(KalmanData *data, double sensorValue) {
    double newOffset = 0;  //新误差
    double zenyi = 0;
    double tmp = 0;
    double tmpv = 0;
    double ret = 0;
    kalmanFilterYuce(data);
    newOffset = sqrt(data->yuceNoise*data->yuceNoise+data->zuiyouNoise*data->zuiyouNoise);
    tmp = newOffset*newOffset;
    zenyi = sqrt(tmp/(tmp+data->sensorNoise*data->sensorNoise));
    tmpv = zenyi*(sensorValue-data->yuceValue);
    ret = data->yuceValue+tmpv;
    data->zuiyouNoise = sqrt((1-zenyi)*tmp);
	  data->yuceValue = ret;
	  if(ret>0 && ret>data->maxOffset) {
			ret = data->maxOffset;
		} else if(ret<0 && ret<-data->maxOffset){
			ret = -data->maxOffset;
		}
    return ret;
}

void kalmanUpdateControl(int32_t dacValue, int32_t offset,int sum) {
	if(g_preDacValue>0 && ABS(g_preDacValue-g_ppreDacValue)>10000 && sum > 2) {
		g_ctvalue = (double)(g_preOffset - offset)/(g_preDacValue-g_ppreDacValue);
		g_ctvalueSum+=g_ctvalue;
		g_ctvalueCount++;
		g_ctvalue = g_ctvalueSum/g_ctvalueCount;
		printf("offset=%d dacoff=%d ctvalue = %lf\n",offset - g_preOffset,g_preDacValue-g_ppreDacValue,g_ctvalue);
	}
	
	g_ppreDacValue = g_preDacValue;
	g_preDacValue = dacValue;
	g_preOffset = offset;
}

void kalmanUpdate(KalmanData *data, uint8_t lock) {
	static float k = 0.001;
	static int32_t factor = 9000;
	#define MAXFACTOR 80.0f
	int32_t agetime = g_Ocxo_info.ocxo_agetime * 100;
	if(agetime== 0)
	{
		agetime = 9000;
	}
	if(factor != agetime)
	{
		factor = agetime;
		k = -(MAXFACTOR - g_Ocxo_info.lowNoise) / factor;
		DBGI("new factor k = %f\n", k);
	}
	data->seconds++;
	float x = data->seconds;
	data->maxOffset = 1000;
	//0-10min
	if(data->seconds%20 == 0) {
		if(data->seconds < (agetime/data->rate)) {
			data->yuceNoise = k*x*data->rate+MAXFACTOR;
		}
		else
		{
			data->yuceNoise = g_Ocxo_info.lowNoise;
		}
		if(data->yuceNoise < g_Ocxo_info.lowNoise)
		{
			if(!lock) {
				data->yuceNoise = g_Ocxo_info.lowNoise;
			} else {
				data->yuceNoise = g_Ocxo_info.lowNoise * 0.7f;
			}
		}
	}
	
}

void kalmanInit(KalmanData *data) {
	g_ctvalue = 0;
	g_ctvalueCount = 0;
	g_ctvalueSum = 0;
	g_ppreDacValue = 0;
	g_preDacValue = 0;
	data->yuceValue = 0;
  data->zuiyouNoise = 20;
  data->sensorNoise = 400;
	data->rate = 1;
}

void kalmanUpdateRate(KalmanData *data)
{
	data->rate = 1/TimeUpdateRate();
}
